#include "Pyramid.h"
#include <cassert>
#include <math3d.h>

Pyramid::Pyramid(void)
{
	m_physicsObject = new ImpossiblePhysics::PhysicsObj();
	m_physicsObject->idString = ImpossiblePhysics::ENV_OBJECT;

	m3dLoadIdentity44(m_matRotation);
}


Pyramid::~Pyramid(void)
{
	// These should always exist before destruction
	assert(m_physicsObject && m_physicsObject->body);

	delete m_physicsObject->body->getMotionState();
	delete m_physicsObject->body->getCollisionShape();

	delete m_physicsObject->body;
	delete m_physicsObject;
}

void Pyramid::Update(float_t argDelta)
{

}

void Pyramid::SetRotation(float_t argRotation, float_t argAxisX, float_t argAxisY, float_t argAxisZ)
{
	m3dRotationMatrix44(m_matRotation, argRotation, argAxisX, argAxisY, argAxisZ);
}

void Pyramid::Draw() const
{
	glPushMatrix();
	float_t _radius = ((btConeShape*)m_physicsObject->body->getCollisionShape())->getRadius();
	float_t _height = ((btConeShape*)m_physicsObject->body->getCollisionShape())->getHeight();
	float_t _halfHeight = _height / 2;

	btTransform _transform;
	float_t _mat[16];

	m_physicsObject->body->getMotionState()->getWorldTransform(_transform);
	_transform.getOpenGLMatrix(_mat);
	glMultMatrixf(_mat);

	// Rotate this pyramid
	glMultMatrixf(m_matRotation);

	// Draw pyramid

	glBegin(GL_TRIANGLES);

	
	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, _halfHeight, 0.0f); // top vert
	glVertex3f(-_radius, -_halfHeight, _radius);
	glVertex3f(_radius, -_halfHeight, _radius);

	glColor3f(1.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, _halfHeight, 0.0f); // top vert
	glVertex3f(-_radius, -_halfHeight, -_radius);
	glVertex3f(-_radius, -_halfHeight, _radius);

	glColor3f(0.0f, 1.0f, 1.0f);
	glVertex3f(0.0f, _halfHeight, 0.0f); // top vert
	glVertex3f(_radius, -_halfHeight, -_radius);
	glVertex3f(-_radius, -_halfHeight, -_radius);

	glColor3f(1.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, _halfHeight, 0.0f); // top vert
	glVertex3f(_radius, -_halfHeight, _radius);
	glVertex3f(_radius, -_halfHeight, -_radius);



	glEnd();
	glPopMatrix();
	
}

void Pyramid::Draw2D() const
{

}